#include <unistd.h>
#include <sys/time.h>
#include <sys/types.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>

#include <linux/input.h>
#include <linux/joystick.h>

#include "RPi_UART.h"

// グローバル変数宣言。
unsigned long sync_usec = 20 * 1000;
int fd_j, fd_u;

// 関数プロトタイプ宣言。


#define NAME_LENGTH 128

int main (int argc, char **argv)
{
	unsigned char axes = 2;
	unsigned char buttons = 2;

	short position = 0;
	unsigned char uart_sBuffer[16];
	char *uart_dev = "/dev/ttyAMA0";
	int i;
	struct timeval tv;
	unsigned long start_usec;
	unsigned long end_usec;
	unsigned long use_usec;

	if (argc < 2 || argc > 3 || !strcmp("--help", argv[1])) {
		puts("");
		puts("Usage: HelloCmdServo <joystick device likes /dev/input/js0>");
		puts("");
		return 1;
	}

	// ジョイスティックファイルオープン。
	if ((fd_j = open(argv[argc - 1], O_RDONLY)) < 0) {
		perror("HelloCmdServo: open joystick error");
		return 1;
	}

	// UARTの初期化。
	fd_u = UART_open_port(uart_dev);

	printf("Starting Servo Control...\n");

	// トルクOnコマンドをサーボ(UART)へ送信。
	/// コマンド構成。
	//// ヘッダ。0xFAAF 固定。
	uart_sBuffer[0] = 0xfa;
	uart_sBuffer[1] = 0xaf;
	//// 対象サーボID。255(0xFF)で全サーボへ。
	uart_sBuffer[2] = 0x01;
	//// 制御フラグ。今回0x00で。
	uart_sBuffer[3] = 0x00;
	//// 書き込みメモリマップ（レジスタ）アドレス。トルクOn/Offは0x24。
	uart_sBuffer[4] = 0x24;
	/// データ長は1バイト。
	uart_sBuffer[5] = 1;
	/// 対象サーボ数。ショートパケット時は1。
	uart_sBuffer[6] = 1;
	/// メモリマップに書き込むデータ。
	uart_sBuffer[7] = 0x01;
	/// チェックサム。
	uart_sBuffer[8] = 0x24;
	/// 送信。
	for (i = 0; i < 9; i++)
	{
		UART_putc(uart_sBuffer[i], fd_u);
	}

	int *axis;
	char *button;
	struct js_event js;

	axis = calloc(axes, sizeof(int));
	button = calloc(buttons, sizeof(char));

	// メインループ。
	while (1) {
		gettimeofday(&tv, NULL);
		start_usec = tv.tv_sec * 1000 * 1000 + tv.tv_usec;

		if (read(fd_j, &js, sizeof(struct js_event)) != sizeof(struct js_event)) {
			perror("\nHelloJoystick: error reading");
			return 1;
		}

		switch(js.type & ~JS_EVENT_INIT) {
		case JS_EVENT_BUTTON:
			button[js.number] = js.value;
			break;
		case JS_EVENT_AXIS:
			axis[js.number] = js.value;
			break;
		}

		printf("\r");

		if (axes)
		{
			// サーボポジションに変換。
			position = axis[0] / 40;

			// Axes1 = コントローラ左側アナログスティックのX軸の値を取得。
			printf("Axes X: %6d, Position %4d ", axis[0], position);

			// UARTへの送信データ構成。
			/// ヘッダ。0xFAAF 固定。
			uart_sBuffer[0] = 0xfa;
			uart_sBuffer[1] = 0xaf;
			/// 対象サーボID。255(0xFF)で全サーボへ。
			uart_sBuffer[2] = 0x01;
			/// 制御フラグ。今回0x00で。
			uart_sBuffer[3] = 0x00;
			/// 書き込みメモリマップ（レジスタ）アドレス。目標位置は0x1E&0x1F。
			uart_sBuffer[4] = 0x1e;
			/// データ長。0x1Eと0x1Fの2バイト。
			uart_sBuffer[5] = 2;
			/// 対象サーボ数。ショートパケット時は1。
			uart_sBuffer[6] = 1;
			/// メモリマップに書き込むデータ。リトルエンディアン。
			uart_sBuffer[7] = (unsigned char)(position & 0x00ff);
			uart_sBuffer[8] = (unsigned char)((position >> 8) & 0x00ff);
			/// チェックサム。
			uart_sBuffer[9] = uart_sBuffer[2];
			for (i = 3; i < 9; i++)
			{
				uart_sBuffer[9] ^= uart_sBuffer[i];
			}

			for (i = 0; i < 10; i++)
			{
				UART_putc(uart_sBuffer[i], fd_u);
			}

		}

		fflush(stdout);

		gettimeofday(&tv, NULL);
		end_usec = tv.tv_sec * 1000 * 1000 + tv.tv_usec;
		use_usec = end_usec - start_usec;

		if (sync_usec > use_usec)
			usleep(sync_usec -use_usec);
	}

	return -1;
}
